/*
 * Center then drive to the end of the hall, 
 * turn around and return to the origional position.
 *
 * Current: check for zero point turn.
 */

#include <libplayerc++/playerc++.h>
#include <iostream>

#include "args.h"

#define RAYS 32

double fixAngle(int rLoc, int lLoc, double oldturn, int count);

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;
    
    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);
    //SonarProxy sp(&robot, gIndex);
    std::cout << robot << std::endl;

    pp.SetMotorEnable (true);
   
    double newspeed = 0;
    double newturnrate = 0;
    double minR = 5.6;
    double minL = 5.6;
    int lloc = 0;
    int rloc = 0;
    double tempA;
    double tempB;
    double history[5];
    int isOpen=1;
    //int temp;

    robot.Read(); //here so GetRange doesn't segfault
      
    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();

      //Get minimum range reading on right and left sides 
      //printf("get new locations.\n");
      uint count = lp.GetCount();
      minR = 5.6;
      minL = 5.6;
      for (uint j = 0; j < count/2; ++j){  // find the minimum on the right side
	if (minR > lp[j]){
	  minR = lp[j];
	  rloc=j;
	}
      }
      
      for (uint j = count/2; j < count; ++j){ // find the minimum on the left side
	if (minL > lp[j]){
	  minL = lp[j];
	  lloc=j;
	}
      }
      
      /* Using only the certain sensors */
      double front = lp[327];
      double right = /*(lp[109] + lp[110])/2; */lp[218];
      double left = /*(lp[545] + lp[546])/2; */lp[437];
      bool turning = false;
      
      if(right <= 0.5){
	isOpen=0;
	newspeed = 0.0;
	newturnrate = 0.375;
      }
      else if(left <= 0.5){
	isOpen=0;
	newspeed = 0.0;
	newturnrate = -0.375;
      }
      else if(right <= 1.0){
	isOpen=0;
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      else if(left <= 1.0){
	isOpen=0;
	newspeed = 0.2;
	newturnrate = -0.375;
      }
      else if(front <= 1.5){
	isOpen=0;
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      else{
	  isOpen=1;
	newspeed = 0.5;
	newturnrate = 0.0;
      }
      
      std::cout << "Front: " << front << std::endl
		<< "Right: " << right << "  rloc: " << rloc << "   minR: " << minR << std::endl
		<< "Left: " << left << "  lloc: " << lloc << "   minL: " << minL << std::endl
		<< "isOpen: " << isOpen << std::endl;
	tempA=0;
	tempB=0;
	// check for max
	for(int i=105;i<115;i++){
	  if(tempA<lp[i])
	    tempA=lp[i];	
	}
	for(int i=540;i<550;i++){
	  if(tempB<lp[i])
	    tempB=lp[i];	
	}
	if(tempA>=5.0 || tempB>5.0)
	  isOpen=0;
	if(isOpen==1)
	  newturnrate=fixAngle(rloc,lloc,newturnrate,655);
	std::cout << "newspeed: " << newspeed << std::endl
		  << "newturn: " << newturnrate << std::endl;
	pp.SetSpeed(newspeed, newturnrate);
	
    }
  }
  
  catch (PlayerCc::PlayerError e)
    {
      std::cerr << e << std::endl;
      return -1;
    }
}

double fixAngle(int rLoc, int lLoc, double oldturn, int count){
  /* Fixes the robots angle from the wall based
     on the location of the closest objects on
     the left and the right 
     
     input: rLoc - right location
     lLoc - left location
     oldturn - the old turn rate
     count - number of samples
     
     output: newturnrate - in proper units
     
     other: theta - the angle required to turn
  */
  double newturnrate;
  double theta;
  
  if((rLoc-110)>1){
    // initiate right turn
    std::cout << "Aye aye Captain, Right Turn!" << std::endl;
    theta= (rLoc*270/count)-45; // sample * degree / sample - degree
    newturnrate=1*theta*0.75/90;  // degree * turn / degree
    return newturnrate;
  }
  if((rLoc-110)<-1){
    // initiate life turn
    std::cout << "Aye aye Captain, Left turn!" << std::endl;
    theta= 225-(lLoc*270/count);
    newturnrate=-1*theta*0.75/90;
    return newturnrate;
  }
  return oldturn;
}
